#include "static_layer.h"
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

namespace costmap_2d{

StaticLayer::StaticLayer(){

}

StaticLayer::~StaticLayer(){
    
}

void StaticLayer::onInitialize(){

}

void StaticLayer::matchSize(){
    if(!layered_costmap_->isRolling()){
        Costmap2D* master = layered_costmap_->getCostmap2D();
        resizeMap(master->getWidth(), master->getHeight(), master->getResolution(),
                master->getOriginX(), master->getOriginY());
    }
}

unsigned char StaticLayer::interpretValue(unsigned char value){
    // 此三个参数 后续调整
    unsigned char lethal_threshold_ = int(100);
    unsigned char unknown_cost_value_ = int(-1);
    bool trinary_costmap_ = true;

    // check if the static value is above the unknown or lethal thresholds
    if (value == unknown_cost_value_)
        return NO_INFORMATION;
    else if (value >= lethal_threshold_)
        return LETHAL_OBSTACLE;
    else if (trinary_costmap_)
        return FREE_SPACE;

    double scale = (double) value / lethal_threshold_;
    return scale * LETHAL_OBSTACLE;
}

void StaticLayer::setMap(const nav_msgs::OccupancyGridConstPtr& new_map){
    unsigned int width = new_map->info.width, height = new_map->info.height;
    ROS_INFO("Received a %d X %d map at %f m/pix", width, height, new_map->info.resolution);

    Costmap2D* master = layered_costmap_->getCostmap2D();

    if (!layered_costmap_->isRolling() &&
        (master->getWidth() != width ||
        master->getHeight() != height ||
        master->getResolution() != new_map->info.resolution ||
        master->getOriginX() != new_map->info.origin.position.x ||
        master->getOriginY() != new_map->info.origin.position.y)){
        ROS_INFO("Resizing costmap to %d X %d at %f m/pix", width, height, new_map->info.resolution);
        layered_costmap_->resizeMap(width, height, new_map->info.resolution, new_map->info.origin.position.x,
                                    new_map->info.origin.position.y);        
    }

    // 初始化当前图层数据
    resizeMap(width, height, new_map->info.resolution,
        new_map->info.origin.position.x, new_map->info.origin.position.y);
    unsigned int index = 0;
    for (unsigned int i = 0; i < height; ++i){
        for (unsigned int j = 0; j < width; ++j){
            unsigned char value = new_map->data[index];
            costmap_[index] = interpretValue(value);
            ++index;
        }
    }
}

void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
    double* max_x, double* max_y){
    LOG(INFO) << "updateBounds...... start" << endl;    
    double wx, wy;
    // 设置左下角像素坐标
    mapToWorld(0, 0, wx, wy);
    *min_x = std::min(wx, *min_x);
    *min_y = std::min(wy, *min_y);
    // 设置右上角像素坐标
    mapToWorld(width_, height_, wx, wy);
    *max_x = std::max(wx, *max_x);
    *max_y = std::max(wy, *max_y);    
    LOG(INFO) << "[updateBounds] min_x: " << *min_x << ", min_y: " << *min_y << ", max_x: " << *max_x << ", max_y: " << *max_y;
    LOG(INFO) << "updateBounds...... over" << endl; 
}

void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j){
    LOG(INFO) << "updateCosts...... start" << endl; 
    // 局部窗口处理
    if(layered_costmap_->isRolling()){
        unsigned int mx, my;
        double wx, wy;  
        // 获取坐标变换
        geometry_msgs::TransformStamped transform;
        try{
            transform = tf_->lookupTransform(layered_costmap_->getGlobalFrameID(), 
                layered_costmap_->getGlobalFrameID(), ros::Time(0));
        }
        catch (tf2::TransformException ex){
            ROS_ERROR("%s", ex.what());
            return;
        }
        tf2::Transform tf2_transform;
        tf2::convert(transform.transform, tf2_transform);
        // 转换位姿
        for (unsigned int i = min_i; i < max_i; ++i){
            for (unsigned int j = min_j; j < max_j; ++j){
                // 获取局部地图真实坐标
                layered_costmap_->getCostmap2D()->mapToWorld(i, j, wx, wy);
                // 转换为本图层下的地图真实坐标
                tf2::Vector3 p(wx, wy, 0);
                p = tf2_transform*p;  
                // 校验本图层下的地图像素坐标
                if(worldToMap(p.x(), p.y(), mx, my)){
                    // 将本图层代价值复制到master_grid
                    master_grid.setCost(i, j, getCost(mx, my));
                }   
            }           
        }
    }
    else{
        unsigned char* master_array = master_grid.getCharMap();
        unsigned int span = master_grid.getWidth();
    
        LOG(INFO) << "master_array size: " << (master_array == NULL)
            << ", costmap_ size: " << (costmap_ == NULL);
      
        for (int j = min_j; j < max_j; j++)
        {
          unsigned int it = j * span + min_i;
          for (int i = min_i; i < max_i; i++)
          {
            if (costmap_[it] == costmap_2d::NO_INFORMATION){
              it++;
              continue;
            }
      
            unsigned char old_cost = master_array[it];
            if (old_cost == costmap_2d::NO_INFORMATION || old_cost < costmap_[it])
              master_array[it] = costmap_[it];
            it++;
          }
        }        
    }
    LOG(INFO) << "updateCosts...... over" << endl; 
}


}   // namespace costmap_2d